import { Plane } from "@babylonjs/core/Maths/math.plane";
import { Vector3 } from "@babylonjs/core/Maths/math.vector";
import { BabylonContainer } from "../base/babylon-container";


const _tV0 = new Vector3();
const _tV1 = new Vector3();
const _tV2 = new Vector3();
const _tV3 = new Vector3();
const _tV4 = new Vector3();
const _tV5 = new Vector3();
const _tV6 = new Vector3();
const _tV7 = new Vector3();
const _tA0: Array<any> = [];

const DEGREE = Math.PI / 180;
const RADIAN = 180 / Math.PI;
const MIN_FLOAT = 1e-12;
const MAX_FLOAT = 1e12;

/**
 * 射线到多段线的最近信息对象
 * @param minDistance 最小距离
 * @param previousIndex 前一顶点索引
 * @param previousIndex 后一顶点索引
 */
interface IRayClosestToSplineInfo {
    minDistance: number,
    previousIndex: number,
    nextIndex: number
}


/**
 * 计算相关的方法
 */
class Calculation {

    /**
     * 两向量之间的角度
     * @param fromVec 向量1
     * @param toVec 向量2
     * @param isDegree 是否以度计算
     * @returns 角度
     */
    public static angleOfVectors(fromVec: Vector3, toVec: Vector3, isDegree?: boolean): number {
        const nor = Vector3.CrossToRef(fromVec, toVec, _tV0);
        nor.normalize();
        const ang = Vector3.GetAngleBetweenVectors(fromVec, toVec, nor);
        return isDegree ? ang * RADIAN : ang;
    }

    /**
     * 计算点集的范围值
     * @param points 点集
     * @param maxOut 最大值
     * @param minOut 最小值
     */
    public static boundingMaxMin(points: Vector3[], maxOut: Vector3, minOut: Vector3): void {
        for (let i = 0; i < points.length; i++) {
            const p = points[i];
            if (i === 0) {
                maxOut.copyFrom(p);
                minOut.copyFrom(p);
                continue;
            }
            if (maxOut.x < p.x) maxOut.x = p.x;
            if (maxOut.y < p.y) maxOut.y = p.y;
            if (maxOut.z < p.z) maxOut.z = p.z;

            if (minOut.x > p.x) minOut.x = p.x;
            if (minOut.y > p.y) minOut.y = p.y;
            if (minOut.z > p.z) minOut.z = p.z;
        }
    }

    /**
     * 两直线的最近点（两直线的公垂线）
     * @param s1 线1点1
     * @param e1 线1点2
     * @param s2 线2点1
     * @param e2 线2点2
     * @param sOut 线1上最近点
     * @param eOut 线2上最近点
     */
    public static closestPointsOfTwoLines(s1: Vector3, e1: Vector3, s2: Vector3, e2: Vector3, sOut: Vector3, eOut: Vector3): void {
        const p1 = s1;
        const d1 = _tV0.copyFrom(e1).subtractInPlace(s1).normalize();
        const p2 = s2;
        const d2 = _tV1.copyFrom(e2).subtractInPlace(s2).normalize();
        const crs = Vector3.CrossToRef(d1, d2, _tV2);
        if (crs.equalsWithEpsilon(Vector3.ZeroReadOnly, MIN_FLOAT)) {
            sOut.copyFrom(p1);
            eOut.copyFrom(p2);
            return;
        }
        const v1a = _tV3.copyFrom(p2).subtractInPlace(p1);
        const v2a = _tV4.copyFrom(v1a);
        const v1b = Vector3.CrossToRef(v1a, d2, v1a);
        const v1c = Vector3.CrossToRef(d1, d2, _tV5);
        const v2c = v1c;
        const v1d = Vector3.Dot(v1b, v1c);
        const v1e = v1c.lengthSquared();
        const t1 = v1d / v1e;
        const v2b = Vector3.CrossToRef(v2a, d1, v2a);
        const v2d = Vector3.Dot(v2b, v2c);
        const v2e = v2c.lengthSquared();
        const t2 = v2d / v2e;
        d1.scaleInPlace(t1).addInPlace(p1);
        d2.scaleInPlace(t2).addInPlace(p2);
        sOut.copyFrom(d1);
        eOut.copyFrom(d2);
    }

    /**
     * 射线与线段的最近点
     * @param origin 射线原点
     * @param direction 射线方向
     * @param v0 线段起点
     * @param v1 线段终点
     * @param pointOnRay 射线上的最近点
     * @param pointOnSegment 线段上的最近点
     * @returns 最近的距离（平方）
     */
    public static closestPointOfRayAndSegment(origin: Vector3, direction: Vector3, v0: Vector3, v1: Vector3, pointOnRay?: Vector3, pointOnSegment?: Vector3): number {
        const _segCenter = _tV0.copyFrom(v0).addInPlace(v1).scaleInPlace(0.5);
        const _segDir = _tV1.copyFrom(v1).subtractInPlace(v0).normalize();
        const _diff = _tV2.copyFrom(origin).subtractInPlace(_segCenter);
        const segExtent = Vector3.Distance(v0, v1) * 0.5;
        const a01 = -Vector3.Dot(direction, _segDir);
        const b0 = Vector3.Dot(_diff, direction);
        const b1 = -Vector3.Dot(_diff, _segDir);
        const c = _diff.lengthSquared();
        const det = Math.abs(1 - a01 * a01);
        let s0, s1, sqrDist, extDet;
        if (det > 0) {
            s0 = a01 * b1 - b0;
            s1 = a01 * b0 - b1;
            extDet = segExtent * det;
            if (s0 >= 0) {
                if (s1 >= -extDet) {
                    if (s1 <= extDet) {
                        const invDet = 1 / det;
                        s0 *= invDet;
                        s1 *= invDet;
                        sqrDist = s0 * (s0 + a01 * s1 + 2 * b0) + s1 * (a01 * s0 + s1 + 2 * b1) + c;
                    } else {
                        s1 = segExtent;
                        s0 = Math.max(0, -(a01 * s1 + b0));
                        sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
                    }
                } else {
                    s1 = -segExtent;
                    s0 = Math.max(0, -(a01 * s1 + b0));
                    sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
                }
            } else {
                if (s1 <= -extDet) {
                    s0 = Math.max(0, -(-a01 * segExtent + b0));
                    s1 = s0 > 0 ? -segExtent : Math.min(Math.max(-segExtent, -b1), segExtent);
                    sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
                } else if (s1 <= extDet) {
                    s0 = 0;
                    s1 = Math.min(Math.max(-segExtent, -b1), segExtent);
                    sqrDist = s1 * (s1 + 2 * b1) + c;
                } else {
                    s0 = Math.max(0, -(a01 * segExtent + b0));
                    s1 = s0 > 0 ? segExtent : Math.min(Math.max(-segExtent, -b1), segExtent);
                    sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
                }
            }
        } else {
            s1 = a01 > 0 ? -segExtent : segExtent;
            s0 = Math.max(0, -(a01 * s1 + b0));
            sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c;
        }

        if (pointOnRay) pointOnRay.copyFrom(direction).scaleInPlace(s0).addInPlace(origin);
        if (pointOnSegment) pointOnSegment.copyFrom(_segDir).scaleInPlace(s1).addInPlace(_segCenter);
        return sqrDist;
    }

    /**
     * 射线与多段线的最近点
     * @param origin 射线原点
     * @param direction 射线方向
     * @param points 多段线顶点集
     * @param rayPointOut 射线上的最近点
     * @param splinePointOut 多段线上的最近点
     * @returns 最近距离（平方）
     */
    public static closestPointOfRayAndSpline(origin: Vector3, direction: Vector3, points: Vector3[], rayPointOut?: Vector3, splinePointOut?: Vector3): IRayClosestToSplineInfo {
        let minDistance = Number.POSITIVE_INFINITY;
        let rp = _tV6, sp = _tV7;
        let previousIndex = -1;
        let nextIndex = -1;
        for (let i = 0; i < points.length - 1; i++) {
            let s = points[i], e = points[i + 1];
            let ds = this.closestPointOfRayAndSegment(origin, direction, s, e, rp, sp);
            if (minDistance > ds) {
                minDistance = ds;
                previousIndex = i;
                nextIndex = i + 1;
                if (rayPointOut) rayPointOut.copyFrom(rp);
                if (splinePointOut) splinePointOut.copyFrom(sp);
            }
        }
        return { minDistance, previousIndex, nextIndex };
    }

    /**
     * 计算点集的递增距离（按序）
     * @param points 点集
     * @param out 距离集合
     * @returns 距离集合
     */
    public static distancesOfPoints(points: Vector3[], out: number[] = []): number[] {
        out.push(0);
        let tempLength = 0;
        for (let i = 1; i < points.length; i++) {
            tempLength += Vector3.Distance(points[i - 1], points[i]);
            out.push(tempLength);
        }
        return out;
    }

    /**
     * 线性对照插值
     * 如：t:0.75, from:0, to:1, node_value_ary:[0,2, 0.5,8, 1,3]
     *    ->return 5.5
     * @param t 比例
     * @param from 比例起始值，一般为0
     * @param to 比例结束值，一般为1
     * @param node_value_ary 对照表,2*i:比例值,2*i+1:插值节点:
     * @returns 比例所对应的取值
     */
    public static valueLinearMapping(t: number, from: number, to: number, node_value_ary: Array<number>): number {
        let ascending = true;
        if (node_value_ary[0] === from && node_value_ary[node_value_ary.length - 2] === to) {
            ascending = from < to;
        } else {
            console.warn('[mapValue] need more aligned data!');
            return t;
        }
        const v = Math.max(Math.min(t, to), from);
        for (let i = 0; i < node_value_ary.length; i += 2) {
            const node = node_value_ary[i];
            if (node === v) {
                return node_value_ary[i + 1];
            }
            let n = node_value_ary[i + 2], s = 0, e = 0;
            const sus = ascending ? (v > node && v < n) : (v < node && v > n);
            if (sus) {
                s = node_value_ary[i + 1];
                e = node_value_ary[i + 3];
                return s + (e - s) * (v - node) / (n - node);
            }
        }
        console.warn('[mapValue] mapping failed!');
        return t;
    }

    /**
     * 按长度比例在点集中取点
     * @param ratio 比例
     * @param points 点集
     * @param pointOut 输出点
     * @param indicesOut 输出索引
     */
    public static ratioPointOfPointsByLength(ratio: number, points: Vector3[], pointOut?: Vector3, indicesOut?: [number, number]): void {
        const out = pointOut || new Vector3();
        const indices = indicesOut || [];
        _tA0.length = 0;
        const lengths = this.distancesOfPoints(points, _tA0);
        const length = lengths[lengths.length - 1];
        const len = length * (ratio % 1);
        for (let i = 0; i < lengths.length; i++) {
            if (lengths[i] === len) {
                out.copyFrom(points[i]);
                if (i === 0) {
                    indices[0] = 0;
                    indices[1] = 1;
                } else {
                    indices[0] = i - 1;
                    indices[1] = i;
                }

            } else if (lengths[i] > len && lengths[i - 1] < len) {
                const rat = (lengths[i] - len) / (lengths[i] - lengths[i - 1]);
                out.copyFrom(points[i - 1]).subtractInPlace(points[i]);
                out.scaleInPlace(rat).addInPlace(points[i]);
                indices[0] = i - 1;
                indices[1] = i;
            }
        }
    }

    /**
     * 按索引比例在点集中取点
     * @param ratio 比例
     * @param points 点集
     * @param pointOut 输出点
     * @param indicesOut 输出索引
     */
    public static ratioPointOfPointsByRatio(ratio: number, points: Vector3[], pointOut?: Vector3, indicesOut?: [number, number]): void {
        const out = pointOut || new Vector3();
        const indices = indicesOut || [];
        const count = points.length - 1;
        if (count < 1) {
            out.copyFrom(points[0]);
            indices[0] = 0;
            indices[1] = 0;
            return;
        }
        const u = 1 / count;
        const s = ratio / u;
        const idx = Math.floor(s);
        const rdx = s - idx;
        if (idx >= count) {
            out.copyFrom(points[count]);
            indices[0] = count - 1;
            indices[1] = count;
        } else {
            out.copyFrom(points[idx + 1]).subtractInPlace(points[idx]).scaleInPlace(rdx).addInPlace(points[idx]);
            indices[0] = idx + 1;
            indices[1] = idx;
        }
    }

    /**
     * 一个向量投影到另一个向量
     * @param source 要投影的向量
     * @param target 要投影到的向量
     * @param output 投影向量
     * @returns 投影向量
     */
    public static projectOnVector(source: Vector3, target: Vector3, output: Vector3): Vector3 {
        const denominator = target.lengthSquared();
        if (denominator === 0) return output.set(0, 0, 0);
        const scalar = Vector3.Dot(target, source) / denominator;
        output.copyFrom(target).scaleInPlace(scalar);
        return output;
    }

    /**
     * 将z向上的向量(右x前y上z)转换为y向上的向量
     * @param x 向量x
     * @param y 向量y
     * @param z 向量z
     * @param isRight 指定使用右手系（不指定将使用容器配置，在容器初始化之前使用最好指定，否则将始终为容器默认配置）
     * @returns y向上的向量
     */
    public static vectorZ(x: number, y: number, z: number, isRight?: boolean): Vector3 {
        const isLeft = isRight === undefined ? (!BabylonContainer.isRightHand) : (!isRight);
        return new Vector3(isLeft ? -x : x, z, -y);
    }

    /**
     * 用法线及过平面点设置平面
     * @param origin 过平面点
     * @param normal 法线
     * @param result 要设置的平面
     * @returns 要设置的平面
     */
    public static setPlaneFromPositionAndNormal(origin: Vector3, normal: Vector3, result: Plane) {
        normal.normalize();
        result.normal = normal;
        result.d = -(normal.x * origin.x + normal.y * origin.y + normal.z * origin.z);
        return result;
    }


}


export { Calculation, DEGREE, MIN_FLOAT, MAX_FLOAT, RADIAN };
export type { IRayClosestToSplineInfo };